#include "main.h"
#include "usbd_cdc_if.h"
#include "stdio.h"
#include "math.h"
#include <string.h>

extern TIM_HandleTypeDef htim3;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
extern UART_HandleTypeDef huart6;

typedef struct
{
  float setpoint;               /*设定值*/
  float kp;                     /*比例系数*/
  float ki;                     /*积分系数*/
  float kd;                     /*微分系数*/
  float lasterror;              /*前一拍偏差*/
  float preerror;               /*前两拍偏差*/
  float deadband;               /*死区*/
  float result;                 /*PID控制器计算结果*/
  float output;                 /*输出值0-100%*/
  float maximum;                /*输出值上限*/
  float minimum;                /*输出值下限*/
  float errorabsmax;            /*偏差绝对值最大值*/
  float errorabsmin;            /*偏差绝对值最小值*/
  float alpha;                  /*不完全微分系数*/
  float derivative;              /*微分项*/
  float integralValue;          /*积分累计量*/
	float LowSpeedValue;					/*低速区间*/
}CLASSICPID;

typedef struct{
	
	float spd_set;//速度设置
	float direction;//方向设置
	
}spd;

typedef struct
{
     int16_t  Input;
     int16_t  Output[2];
     int32_t  FilterTf;		
     int32_t  FilterTs;
     int32_t  Kr;
     int32_t  Ky;
	
}low_filter;	//一阶低通滤波

static low_filter v;
static CLASSICPID vPID;
static spd pd;

uint8_t rev_data[64];
uint8_t charbuf[200];
volatile uint16_t ctrl_data[16];

uint8_t sbus_len=0;
uint8_t sbus_recv_end_flag=0;
uint8_t sbus_data[50];

uint8_t imu_len=0;
uint8_t imu_recv_end_flag=0;
uint8_t imu_data[42];

uint8_t gps_len=0;
uint8_t gps_recv_end_flag=0;
uint8_t gps_data[300];

uint8_t debug_len=0;
uint8_t debug_recv_end_flag=0;
uint8_t debug_data[300];

double pulse2kmh(int32_t pulse)
{
	return (double)pulse * 0.011;
}

int32_t kmh2pulse(double speed)
{
	int32_t pulse = (int32_t)(speed / 0.011);
		
	return pulse;
}

void low_filter_init(void){
	
		 v.FilterTs = 1;
		 v.FilterTf = 3;
		
     v.Kr = v.FilterTs*1024/(v.FilterTs + v.FilterTf);
     v.Ky = v.FilterTf*1024/(v.FilterTs + v.FilterTf);
}

int16_t low_filter_calc(int16_t pulse){

	int32_t tmp = 0;
	
	v.Input = pulse;

	tmp = ((int32_t)v.Kr*v.Input + v.Ky*v.Output[1])/1024;
	
	if(tmp>32767){
		tmp = 32767;
	}
	
	if( tmp < -32768){
		tmp = -32768;
	}
	
    v.Output[0] = (int16_t)tmp;
    v.Output[1] = v.Output[0];
	return v.Output[0];
}


/* 变积分系数处理函数，实现一个输出0和1之间的分段线性函数           */
/* 当偏差的绝对值小于最小值时，输出为1；当偏差的绝对值大于最大值时，输出为0   */
/* 当偏差的绝对值介于最大值和最小值之间时，输出在0和1之间现行变化    */
/* float error，当前输入的偏差值                                         */
/* float absmax，偏差绝对值的最大值                                      */
/* float absmin，偏差绝对值的最小值                                      */
float VariableIntegralCoefficient(float error,float absmax,float absmin)
{
  float factor=0.0;
 
 if(fabs(error)<=absmin)
  {
   factor=1.0;
  }
  else if(fabs(error)>absmax)
  {
   factor=0.0;
  }
  else
  {
   factor=(absmax-fabs(error))/(absmax-absmin);
  }
 
  return factor;
}


int16_t PIDRegulator(int16_t pv)
{
	static uint8_t inc_state=0,inc_count=0;
  float thisError;
  float result;
  float factor;
	
	vPID.setpoint = (float)pv;

  thisError=vPID.setpoint-(float)inc_data; //得到偏差值
  result=vPID.result;
	
	if(abs((int)vPID.setpoint) < vPID.LowSpeedValue) //低速区间内降低变积分系数上限，减小积分超调作用
	{
		vPID.errorabsmax = 1000;
	}
	else
	{
		vPID.errorabsmax = 2000;
	}
	
	if(abs((int)vPID.setpoint) < vPID.deadband) //死区内积分清零 以免影响停车
	{
		vPID.integralValue = 0;
	}
	else
	{
		//变积分系数获取
		factor=VariableIntegralCoefficient(thisError,vPID.errorabsmax,vPID.errorabsmin);
		vPID.integralValue += factor*thisError;
	}
	
	{	//编码器掉落检测
		if(inc_data != 0)
		{
			inc_count = 0;
			inc_state = 0;
		}
		if(fabs(vPID.result * 10) > fabs(thisError)/2+100 && inc_data == 0)
			inc_count++;
		if(inc_count>20)
			inc_state = 2;
		if(inc_state == 2)
			return 0;
	}	
	//计算微分项增量带不完全微分
	vPID. derivative = vPID.kd*(1-vPID.alpha)*(thisError-vPID.lasterror +vPID.alpha*vPID. derivative);

	result=vPID.kp*thisError+vPID.ki*vPID. integralValue +vPID. derivative;
    
  /*对输出限值，避免超调和积分饱和问题*/
  if(result>=vPID.maximum)
  {
    result=vPID.maximum;
  }

  if(result<=vPID.minimum)
  {
    result=vPID.minimum;
  }

  vPID.preerror=vPID.lasterror;  //存放偏差用于下次运算
  vPID.lasterror=thisError;
  vPID.result=result;

	vPID.output=result;

	
	return (int16_t)vPID.output;
}

int _init_PID(void)
{
	
		vPID.LowSpeedValue = 2000;
	  vPID.deadband = 500;
		vPID.kp = 0.080;
		vPID.ki = 0.007;
		vPID.kd = 0.020;
		vPID.alpha = 0.010;
		vPID.errorabsmin = 100;
		vPID.errorabsmax = 2000;
	
		vPID.maximum = 200.0;
		vPID.minimum =-200.0;
	
		low_filter_init();
}

void pwm_ctrl(uint8_t ch,int16_t pwm)
{
		static uint16_t ch1,ch2;
	
		if(ch == 1) ch1 = 76 + (int16_t)((float)pwm/100 * 10);
	
		if(ch == 2) ch2 = 76 + (int16_t)((float)pwm/100 * 10);
	
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, ch1);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, ch2);
}

void dir_crtl(float angle)
{
		int16_t pwm = (int16_t)(angle*4);
		pwm_ctrl(1,pwm);
}

void spd_ctrl(float speed)
{
		static int16_t pwm_out,pulse;

		if(speed > 100) speed = 100;
		if(speed <-100) speed =-100;
	
		pulse = kmh2pulse(speed);
	
		pwm_out = PIDRegulator(pulse);
		
		if(pwm_out > 200) pwm_out = 200;
		if(pwm_out <-200) pwm_out =-200;
	
		pwm_ctrl(2,pwm_out);
		
		printf("%d,%d,%d\n",pulse,inc_data,pwm_out*10);
		
		HAL_Delay(10);
		
}

int sbus_read_parse(uint16_t start,uint16_t *val,uint8_t *databuf)
{

    //按11bits解析遥控器通道
    val[0] = ((databuf[start + 1] | databuf[start + 2] << 8) & 0x07FF);
    val[1] = ((databuf[start + 2] >> 3 | databuf[start + 3] << 5) & 0x07FF);
    val[2] = ((databuf[start + 3] >> 6 | databuf[start + 4] << 2 | databuf[start + 5] << 10) & 0x07FF);
    val[3] = ((databuf[start + 5] >> 1 | databuf[start + 6] << 7) & 0x07FF);
    val[4] = ((databuf[start + 6] >> 4 | databuf[start + 7] << 4) & 0x07FF);
    val[5] = ((databuf[start + 7] >> 7 | databuf[start + 8] << 1 | databuf[start + 9] << 9) & 0x07FF);
    val[6] = ((databuf[start + 9] >> 2 | databuf[start + 10] << 6) & 0x07FF);
    val[7] = ((databuf[start + 10] >> 5 | databuf[start + 11] << 3) & 0x07FF);
    val[8] = ((databuf[start + 12] | databuf[start + 13] << 8) & 0x07FF);
    val[9] = ((databuf[start + 13] >> 3 | databuf[start + 14] << 5) & 0x07FF);
    val[10] = ((databuf[start + 14] >> 6 | databuf[start + 15] << 2 | databuf[start + 16] << 10) & 0x07FF);
    val[11] = ((databuf[start + 16] >> 1 | databuf[start + 17] << 7) & 0x07FF);
    val[12] = ((databuf[start + 17] >> 4 | databuf[start + 18] << 4) & 0x07FF);
    val[13] = ((databuf[start + 18] >> 7 | databuf[start + 19] << 1 | databuf[start + 20] << 9) & 0x07FF);
    val[14] = ((databuf[start + 20] >> 2 | databuf[start + 21] << 6) & 0x07FF);
    val[15] = ((databuf[start + 21] >> 5 | databuf[start + 22] << 3) & 0x07FF);

	 for(uint16_t i=0;i<15;i++)
	 {
			if(abs(val[i] - 2047) > 2050)
				return -1;
	 }
		
    return 0;
}


int sbus_rev_data(void)
{
		if(sbus_recv_end_flag == 1)
		{
			sbus_recv_end_flag = 0;
			if(sbus_data[0]==0x0f && sbus_data[24]==0x00 && sbus_len==25)	//完整的一帧数据
			{
				if(!sbus_read_parse(0,ctrl_data,sbus_data));
					return -1;
			}
//			printf("sbus_data:%d %d len:%d\r\n",ctrl_data[2],ctrl_data[3],sbus_len);
		}
		return 0;
}

void usb_packsand_func(void)
{
	static uint8_t pack_data[14];
	static float speed_f;
	static uint16_t speed,i,sum;
	
	sum = 0;
	
	pack_data[0] = pack_data[1] = 0xaa;
	pack_data[12] = pack_data[13] = 0xbb;
	
	speed_f = pulse2kmh(inc_data);
	speed = (int16_t)speed_f;
	pack_data[2] = (uint8_t)(speed>>8 & 0xff);
	pack_data[3] = (uint8_t)(speed & 0xff);
	pack_data[4] = (uint8_t)((int16_t)(speed_f*100)%100 & 0xff);
	
	pack_data[5] = pack_data[6] =0;
	
	pack_data[7] = (uint8_t)((float)ctrl_data[2]/2047 * 0xff);
	pack_data[8] = (uint8_t)((float)ctrl_data[3]/2047 * 0xff);
	pack_data[9] = (uint8_t)((float)ctrl_data[4]/2047 * 0xff);
	pack_data[10] = (uint8_t)((float)ctrl_data[5]/2047 * 0xff);
	
 	for(i=2;i<=10;i++)
		sum+=pack_data[i];
	
	pack_data[11] = (uint8_t)(sum & 0xff);
	
//	CDC_Transmit_FS(pack_data,sizeof(pack_data));
}

int usb_decode_func(void)
{
	uint16_t i;
	uint32_t sum=0;
	if(rev_data[0] == 0xaa && rev_data[1] == 0xaa && rev_data[12] == 0xbb && rev_data[13] == 0xbb)
	{
		for(i=2;i<=10;i++)
			sum += rev_data[i];
		if((sum&0xff) != rev_data[11]) 	//return -1; //计算校验和
		{	
			sprintf(charbuf,"wrong  data sum is x%  but %x\r\n",sum&0xff,rev_data[11]);
			CDC_Transmit_FS(charbuf,strlen(charbuf));
		}
		
		pd.spd_set = (float)((int16_t)(rev_data[2]<<8 | rev_data[3])) + ((float)(int8_t)rev_data[4]/100);
				
		pd.direction = (float)(int8_t)rev_data[5] + ((float)(int8_t)rev_data[6]/100);
		
		sprintf(charbuf,"spd:%.02lf dir:%.02lf  \r\n",pd.spd_set,pd.direction);
    CDC_Transmit_FS(charbuf,strlen(charbuf));
		
	}
		
	memset(rev_data,0,sizeof(rev_data));
}

void imu_init(void)
{
		uint8_t op_buf[4] = {0xa5,0x15,0xba};
		HAL_UART_Transmit(&huart3,op_buf,3,5);
}

void test_running(void)
{
		float speed;
	
		uint16_t i=0;
	
		_init_PID();
		sbus_rev_data();
		while(1)
		{	
				if(!sbus_rev_data())
				{
						if(ctrl_data[5] > 1000)
						{
							usb_decode_func();
							
							spd_ctrl(pd.spd_set);
							dir_crtl(pd.direction);

						}
						else
						{
							
							if(ctrl_data[3]<1008  && ctrl_data[3] > 1000) ctrl_data[3] = 1004;
							
							{
								static float spd_last,spd_curr,spd_data;
								spd_curr=(float)ctrl_data[3]/2008.0 * 300.0 - 150;
								if(fabs(spd_curr - spd_last)>20)
									spd_data = spd_last;
								else
									spd_data = spd_curr;
								spd_last = spd_curr;
								spd_ctrl(spd_data);
								dir_crtl((float)ctrl_data[2]/2008.0 * 300.0 - 150);
							}
//				if(gps_recv_end_flag == 1)
//				{
//					gps_recv_end_flag = 0;
////					sprintf(charbuf,"ok,data_len:%d bt_1:%x bt_2:%x bt_3:%x bt_4:%x bt_5:%x bt_6:%x bt_7:%x\r\n",gps_len,gps_data[0],gps_data[1],gps_data[2],gps_data[3],gps_data[4],gps_data[5],gps_data[6]);
////					CDC_Transmit_FS(charbuf,strlen(charbuf));
//    			CDC_Transmit_FS(gps_data,strlen(gps_data));
//					memset(gps_data,0,sizeof(gps_data));
//				}
//					printf("wdnmd\r\n");
//					if(debug_recv_end_flag == 1)
//					{
//						debug_recv_end_flag = 0;
//						CDC_Transmit_FS(debug_data,strlen(debug_data));
//						HAL_UART_Transmit(&huart1,debug_data,strlen(debug_data),0xfffe);
//					}
						}//if switch
			
				}	//if ctrldata
	}
}

